#include <QApplication>
#include <QObject>

#include <QHostAddress>

#include <QGraphicsScene>
#include <QGraphicsView>
#include <QRect>

#include <sys/time.h>

#include "server.h"

//QReadWriteLock m_mutexCamera;

Server::Server()
{
    m_speedL = m_speedR = 0;
    m_laserViewer = 0;
    m_cameraViewer = 0;
}

Server::~Server()
{
}

void Server::run()
{
    runThreadTCPData();
//    runThreadVideo();
    runThreadMotor();
    runThreadLaser();
}

void Server::display()
{
//    displayCamera();
//    displayLaser();
//    displayController();
}

void Server::runThreadMotor()
{
    MotorThread *m_motorThread = new MotorThread( this );

    m_motorThread->start();
}

void Server::runThreadVideo()
{
    // Thread to get video streaming via http
    m_mjpegThread = new MJPEGClient( "localhost" );

    QObject::connect( m_mjpegThread, SIGNAL( gotNewImage( const QImage * ) ), this, SLOT( gotNewSensorData( const QImage * ) ) );

    m_mjpegThread->resetPanTitl();
    m_mjpegThread->getVideoFromStreamer();
}

void Server::runThreadLaser()
{
    URGThread *m_laserThread = new URGThread( this );

    m_laserThread->start();
}

void Server::gotNewSensorData( const RS232Data* sensorL, const RS232Data* sensorR )
{
    unsigned long int timeStamp = getCurrentTime();
    //qDebug() << "Odometry timestamp : " << timeStamp << "Speed left: " << sensorL->speed_front << "Speed right: " << sensorR->speed_front;

    //emit dataRobotReady( c)
}

void Server::gotNewSensorData( const QImage* pic )
{
    //m_mutexCamera.lockForRead();

    static bool connected = false;

    if ( !connected )
    {
	connected = true;
	qDebug() << "Camera connected";
    }

    m_camera.m_timeStamp = getCurrentTime();

    //qDebug() << "Camera time stamp: " << m_camera.m_timeStamp;

    m_camera.m_pic = *pic;

    //m_mutexCamera.unlock();
}

void Server::gotNewSensorData( vector<long> laserdata, const RangeSensorParameter& urg_params )
{
    m_laser.m_timeStamp = getCurrentTime();

    for ( int i = 0; i < m_laser.NUM_SHOTS; i++ )
    {
	m_laser.dist[i] = (double)laserdata[i + urg_params.area_min] / 1000.0;

	if ( m_laser.dist[i] <= m_laser.MIN_RANGE )
	    m_laser.dist[i] = 0;
    }

    emit dataLaserReady( &m_laser );

    //qDebug() << "Laser timestamp: " << m_laser.m_timeStamp << " range at 0 degree : " << m_laser.dist[urg_params.area_front] << " m";

    if ( m_laserViewer )
	m_laserViewer->update( m_laserViewer->boundingRect() );
}

void Server::gotNewCommand( int cmd_left, int cmd_right )
{
    m_speedL = cmd_left;
    m_speedR = cmd_right;
}

void Server::displayCamera()
{
    m_cameraViewer = new QwImageViewer( 320, 240 );

    QObject::connect( m_mjpegThread, SIGNAL( gotNewImage( const QImage * ) ), m_cameraViewer, SLOT( refresh( const QImage * ) ) );
    QObject::connect( m_cameraViewer, SIGNAL( cmdReceived(QKeyEvent*) ), m_mjpegThread, SLOT( cmdProcessing(QKeyEvent*) ) );

    m_cameraViewer->show();
}

void Server::displayLaser()
{
    // laser data viewer
    m_scene = new QGraphicsScene( QRect( 0, 0, 500, 500 ) );

    m_laserViewer = new QiLaserScan( m_laser );
    m_laserViewer->setFlag( QGraphicsItem::ItemIsFocusable );

    m_scene->addItem( m_laserViewer );
    m_laserViewer->setPos( 250, 250 );

    //m_laserViewer->setFocus();
    m_scene->setFocusItem( m_laserViewer );

    m_view = new QGraphicsView();
    m_view->setScene( m_scene );
    m_view->show();

    m_view->scale( 1.0, -1.0 );

    //m_scene->addText("Hello, world!");
    //QMatrix XYFlip;
    //XYFlip.setMatrix(0.,1.,1.,0.,0.,0.);
    //m_view->setMatrix(XYFlip);

    // y is up
    //m_view->rotate(90);
    // some initial zoom
    //m_view->scale(5.0,5.0);

    //m_scene->setForegroundBrush(QBrush(Qt::lightGray, Qt::CrossPattern));

}

void Server::displayController()
{
    m_controllerViewer = new QwController();

    connect( m_controllerViewer, SIGNAL( newCommand(int, int) ), this, SLOT(gotNewCommand(int, int) ) );

    m_controllerViewer->show();
}

void Server::runThreadTCPData()
{
    listen( QHostAddress::Any, 9876 );
    qDebug() << "Wifibot server listening on port 9876 ...";
}

void Server::incomingConnection( int descriptor )
{
    TCPDataThread* tcpThread = new TCPDataThread( descriptor );

    connect( tcpThread, SIGNAL( finished() ), tcpThread, SLOT( deleteLater() ) );
    connect( this, SIGNAL( dataLaserReady( LaserData* ) ), tcpThread, SLOT( dataLaserToSend( LaserData* ) ) );
    connect( tcpThread, SIGNAL( commandReceived(int, int) ), this, SLOT( gotNewCommand(int, int) ) );

    tcpThread->start();
}

TCPDataThread::TCPDataThread( int descriptor ) : QThread()
{
    m_socket.abort();

    if ( ! m_socket.setSocketDescriptor( descriptor ) )
    {
	   qDebug() << "ERROR: socket connection problem";
	   return;
    }

    qDebug() << "client connected";

    connect( &m_socket, SIGNAL( readyRead() ), this, SLOT( dataToRead() ) );
}

void TCPDataThread::run()
{
    while ( m_socket.state() == QAbstractSocket::ConnectedState )
    {
	//qDebug() << m_socket.state();
    }

    stop();
}

void TCPDataThread::stop()
{
    m_socket.disconnectFromHost();

    if ( m_socket.state() == QAbstractSocket::UnconnectedState || m_socket.waitForDisconnected(1000) )
	 qDebug("client disconnected");
}

void TCPDataThread::dataLaserToSend( LaserData* laserdata )
{
    if ( m_socket.state() != QAbstractSocket::ConnectedState )
	return;

    QDataStream stream( &m_socket );
    stream.setVersion( QDataStream::Qt_4_0 );

    //stream << "L";

    int dataSize = 0;

    stream << (double)laserdata->MAX_RANGE;
    stream << (double)laserdata->MIN_RANGE;
    stream << (int)laserdata->NUM_SHOTS; // equivalent to qint16 ???
    stream << (double)laserdata->START_ANGLE;
    stream << (double)laserdata->END_ANGLE;
    stream << (double)laserdata->RESOLUTION;

    dataSize += 5*sizeof(double) + sizeof(int);

    for ( int i = 0; i < laserdata->NUM_SHOTS; i++ )
    {
	stream << (double)laserdata->dist[i];
	dataSize += sizeof(double);
    }

    //qDebug() << "laser data size: " << dataSize;

    m_socket.flush();
    m_socket.waitForBytesWritten();
}

void TCPDataThread::dataToRead()
{
    if ( m_socket.state() != QAbstractSocket::ConnectedState )
	return;

    QDataStream stream( &m_socket );
    stream.setVersion( QDataStream::Qt_4_0 );

    while ( ! stream.atEnd() && m_socket.bytesAvailable() >= 2*sizeof(quint16) )
    {
	quint16 cmd_left, cmd_right;
	stream >> cmd_left;
	stream >> cmd_right;

	//qDebug() << cmd_left << cmd_right;

	emit commandReceived( cmd_left, cmd_right );
    }
}

MotorThread::MotorThread( Server* server ) : QThread()
{
    m_server = server;
}

void MotorThread::run()
{
    //m_hCOM=SetupI2CCommPort(0);
    m_hCOM = m_controller.openRS232( "/dev/ttyS0", 19200 );

    qDebug() << "Motor COM: " << m_hCOM;
    if ( m_hCOM == 0 )
    {
       qDebug() << "ERROR: Robot motor problem";

       QApplication::instance()->quit();

       return;
    }

    m_controller.setRS232Motor33f_low_res( 0x00, 0x00 );
    m_controller.setRS232MotorPID33f( 0x00, 0x00, 80, 45, 0, 360 );

    while ( true )
    {
	m_controller.setRS232Motor33f_low_res( m_server->m_speedL, m_server->m_speedR );
	m_controller.getRS232Motor33f( &m_dataL, &m_dataR );

	//unsigned long int timeStamp = getCurrentTime();
	//qDebug() << "Odometry timestamp : " << timeStamp << "Speed left: " << m_dataL.speed_front << "Speed right: " << m_dataR.speed_front;
	
	emit m_server->gotNewSensorData( &m_dataL, &m_dataR );
    }

}

void MotorThread::stop()
{
    if ( m_hCOM != 0 )
    {
	m_controller.stopMotorRS23233f();
	m_controller.closeRS232();
    }
}

URGThread::URGThread( Server* server ) : QThread()
{
    m_server = server;
}

void URGThread::run()
{
    const char device[] = "/dev/ttyACM0";

    m_server->m_laser.NUM_SHOTS = 0;

    if ( ! m_urg.connect( device ) )
    {
	qDebug() << "ERROR: URG laser problem (" << m_urg.what() << ")";

	QApplication::instance()->quit();

	return;
    }
    else
	qDebug() << "URG laser connected";

    m_urg_params = m_urg.parameter();

    //qDebug() << m_urg_params.distance_min << ", " << m_urg_params.area_min << ", " << m_urg_params.area_max << ", " << m_urg_params.area_front << ", " << m_urg_params.area_total;

    m_server->m_laser.MAX_RANGE = m_urg_params.distance_max / 1000.0;
    m_server->m_laser.MIN_RANGE = m_urg_params.distance_min / 1000.0;
    m_server->m_laser.NUM_SHOTS = m_urg_params.area_max - m_urg_params.area_min + 1;
    m_server->m_laser.START_ANGLE = -120.0*M_PI/180.0;
    m_server->m_laser.END_ANGLE = 120.0*M_PI/180.0;
    m_server->m_laser.RESOLUTION = 360.0/m_urg_params.area_total * M_PI/180.0; //(m_laser.END_ANGLE - m_laser.START_ANGLE) / ( m_laser.NUM_SHOTS - 0 );

    m_server->m_laser.dist.resize( m_server->m_laser.NUM_SHOTS );
  
    static vector<long> dist;
    static long timeStamp;

    while ( true )
    {
	int n = m_urg.capture( dist, &timeStamp );

	if (n < 0)
	{
	    qDebug() << "ERROR: URG capture problem (" << m_urg.what() << ")";

	    QApplication::instance()->quit();
	}
	else
	{
	    emit m_server->gotNewSensorData( dist, m_urg_params );
	}
	
    }
}

unsigned long int getCurrentTime()
{
    struct timeval time_d;

    gettimeofday( &time_d, NULL );

    return time_d.tv_sec * 1000000 + time_d.tv_usec;

}


